package hcqs.nav.grid.map.layers;

import hcqs.nav.grid.consts.GridMapConsts;
import hcqs.nav.grid.macros.Cell;
import hcqs.nav.grid.macros.Point;
import hcqs.nav.grid.macros.Pose;
import hcqs.nav.grid.map.algo.GridSimLine;
import hcqs.nav.grid.map.api.Layer;
import hcqs.nav.grid.map.macros.GridMapRectArea;

import java.util.Arrays;
import java.util.List;

public class StaticLayer extends Layer {
    @Override
    public void saveMap(String mapId) {
        // save the map into database or file...
    }

    @Override
    public void changeMap(String mapId) {
        if (mapId.equals(getMapId()))
            return;
        // load the map from database or file...
    }

    @Override
    public void updateMap(Point ltPoint, int width, int height, byte[][] map) {
        increaseGridMap(ltPoint, width, height);
        updateMapWithData(ltPoint, map);
    }

    @Override
    public void refreshMap(String mapId, Point leftTopPoint, byte[][] map) {
        setLeftTopPoint(leftTopPoint);
        this.staticMap = map;
    }

    @Override
    public byte[][] getMapArea(Point ltPoint, int width, int height) {
        if (staticMap == null || width <= 0 || height <= 0)
            return null;
        Cell cellStart = getCellStart(ltPoint);
        Cell cellEnd = getCellEnd(ltPoint, width, height);
        int realWidth = cellEnd.getX() - cellStart.getX();
        int realHeight = cellStart.getY() - cellEnd.getY();
        if (realWidth <= 0 || realHeight <= 0)
            return null;
        byte[][] rst = new byte[realHeight][realWidth];
        for (int row = cellStart.getY(); row < cellStart.getY() + realHeight; row++) {
            for (int col = cellStart.getX(); col < cellStart.getX() + realWidth; col++) {
                rst[row - cellStart.getY()][col - cellStart.getX()] = staticMap[row][col];
            }
        }
        return rst;
    }

    @Override
    public void addPartialMap(String mapId, Pose agvPose, List<Point> obstacles) {
        GridMapRectArea gridMapRectArea = getGridMapPartial(agvPose, obstacles);
        addPartGridMapArea(mapId, gridMapRectArea.getOp(), gridMapRectArea.getArea());
    }

    private void addPartGridMapArea(String agvId, Point p, byte[][] gridMapPart) {
        int widthLeft = 0;
        int widthRight = 0;
        int heightTop = 0;
        int heightBottom = 0;
        if (p.getX() + gridMapPart[0].length * getResolution() > getLeftTopPoint().getX() + staticMap[0].length * getResolution())
            widthRight = (p.getX() - getLeftTopPoint().getX()) / getResolution() + gridMapPart[0].length - staticMap[0].length;
        if (gridMapPart.length * getResolution() - p.getY() > staticMap.length * getResolution() - getLeftTopPoint().getY())
            heightBottom = (getLeftTopPoint().getY() - p.getY()) / getResolution() + gridMapPart.length - staticMap.length;
        if (p.getX() < getLeftTopPoint().getX()) {
            widthLeft = (getLeftTopPoint().getX() - p.getX()) / getResolution();
            getLeftTopPoint().setX(p.getX());
        }
        if (p.getY() > getLeftTopPoint().getY()) {
            heightTop = (p.getY() - getLeftTopPoint().getY()) / getResolution();
            getLeftTopPoint().setY(p.getY());
        }
        increaseGridMap(widthLeft, widthRight, heightTop, heightBottom);
        appendPartGridMapArea(agvId, p, gridMapPart);
    }

    private void appendPartGridMapArea(String agvId, Point p, byte[][] gridMapPart) {
        int xOffset = (p.getX() - getLeftTopPoint().getX()) / GridMapConsts.GRID_LENGTH;
        int yOffset = (getLeftTopPoint().getY() - p.getY()) / GridMapConsts.GRID_LENGTH;
        for (int row = 0; row < gridMapPart.length; row++) {
            for (int col = 0; col < gridMapPart[row].length; col++) {
                if (row + yOffset < 0 || row + yOffset >= staticMap.length
                        || col + xOffset < 0 || col + xOffset >= staticMap[row + yOffset].length) {
                    System.out.println(String.format("Wrong append part grid map area. row: %s, col: %s, yOffset: %s, xOffset: %s", row, col, yOffset, xOffset));
                }
                if (gridMapPart[row][col] < 0) { // 障碍物
                    staticMap[row + yOffset][col + xOffset] = GridMapConsts.GRID_OBSTACLE;
                } else if (gridMapPart[row][col] != 127) { // 可通行
                    staticMap[row + yOffset][col + xOffset] = GridMapConsts.GRID_PASS;
                }
            }
        }
    }

    private void updateMapWithData(Point ltPoint, byte[][] map) {
        Cell cellStart = getCellStart(ltPoint);
        for (int col = 0; col < map.length; col++) {
            for (int row = 0; row < map[col].length; row++) {
                if (map[row][col] == GridMapConsts.GRID_OBSTACLE) {
                    staticMap[row + cellStart.getY()][col + cellStart.getX()] = map[row][col];
                }
            }
        }
    }

    private void increaseGridMap(int widthLeft, int widthRight, int heightTop, int heightBottom) {
        if (widthLeft == 0 && widthRight == 0 && heightTop == 0 && heightBottom == 0)
            return;
        byte[][] target = new byte[staticMap.length + heightTop + heightBottom][staticMap[0].length + widthLeft + widthRight];
        for (int row = 0; row < target.length; row++) {
            Arrays.fill(target[row], GridMapConsts.GRID_UNKNOWN);
        }
        for (int row = 0; row < staticMap.length; row++) {
            System.arraycopy(staticMap[row], 0, target[row + heightTop], widthLeft, staticMap[row].length);
        }
        staticMap = target;
        setLeftTopPoint(new Point(getLeftTopPoint().getX() - widthLeft * getResolution(), getLeftTopPoint().getY() + heightTop * getResolution()));
    }

    private void increaseGridMap(Point p, int width, int height) {
        int widthLeft = 0;
        int widthRight = 0;
        int heightTop = 0;
        int heightBottom = 0;
        if (p.getX() + width > getLeftTopPoint().getX() + staticMap[0].length * getResolution())
            widthRight = (p.getX() - getLeftTopPoint().getX()) / getResolution() + width;
        if (height - p.getY() > staticMap.length * getResolution() - getLeftTopPoint().getY())
            heightBottom = (getLeftTopPoint().getY() - p.getY()) / getResolution() + height;
        if (p.getX() < getLeftTopPoint().getX()) {
            widthLeft = (getLeftTopPoint().getX() - p.getX()) / getResolution();
            getLeftTopPoint().setX(p.getX());
        }
        if (p.getY() > getLeftTopPoint().getY()) {
            heightTop = (p.getY() - getLeftTopPoint().getY()) / getResolution();
            getLeftTopPoint().setY(p.getY());
        }
        increaseGridMap(widthLeft, widthRight, heightTop, heightBottom);
    }

    private Cell getCellEnd(Point ltPoint, int width, int height) {
        Cell cell = new Cell();
        cell.setX(fitX(ltPoint.getX() + width * getResolution()));
        cell.setY(fitY(ltPoint.getY() - height * getResolution()));
        return cell;
    }

    private Cell getCellStart(Point ltPoint) {
        Cell cell = new Cell();
        cell.setX(fitX(ltPoint.getX()));
        cell.setY(fitY(ltPoint.getY()));
        return cell;
    }

    private int fitY(int y) {
        if (y > getLeftTopPoint().getY()) {
            return 0;
        } else if (y < getLeftTopPoint().getY() - getResolution() * staticMap.length) {
            return staticMap.length - 1;
        } else {
            return Math.min((getLeftTopPoint().getY() - y) / getResolution(), staticMap.length - 1);
        }
    }

    private int fitX(int x) {
        if (x < getLeftTopPoint().getX()) {
            return 0;
        } else if (x > getLeftTopPoint().getX() + getResolution() * staticMap[0].length) {
            return staticMap[0].length - 1;
        } else {
            return Math.min((x - getLeftTopPoint().getX()) / getResolution(), staticMap[0].length - 1);
        }
    }

    private GridMapRectArea getGridMapPartial(Pose agvPose, List<Point> obstacles) {
        int angleAntiClockwise = agvPose.getOrient() + GridMapConsts.CAMERA_SIGHT_ANGLE / 2;
        int angleClockwise = agvPose.getOrient()  - GridMapConsts.CAMERA_SIGHT_ANGLE / 2;
        angleAntiClockwise = angleAntiClockwise >= 360000 ? angleAntiClockwise - 360000 : angleAntiClockwise;
        angleClockwise = angleClockwise < 0 ? angleClockwise + 360000 : angleClockwise;
        Point op = null;
        byte[][] area = null;
        int size = GridMapConsts.CAMERA_SIGHT_LENGTH / getResolution();
        getRectMap(agvPose, angleAntiClockwise, angleClockwise, size);
        GridMapRectArea gridMapRectArea = new GridMapRectArea();
        gridMapRectArea.setOp(op);
        for (byte[] bytes : area) {
            Arrays.fill(bytes, GridMapConsts.GRID_UNKNOWN);
        }
        gridMapRectArea.setArea(area);
        // 填充障碍物栅格
        fillObstacles(obstacles, op, gridMapRectArea);
        // 确定起点row,col
        int colStart = fitGridCell(agvPose.getX() - op.getX()) / GridMapConsts.GRID_LENGTH;
        colStart = Math.max(0, colStart);
        int rowStart = fitGridCell(op.getY() - agvPose.getY()) / GridMapConsts.GRID_LENGTH;
        rowStart = Math.max(0, rowStart);
        // 确定边界栅格
        Point edgeCellStart = getPoint(rowStart, colStart, agvPose.getOrient() + GridMapConsts.CAMERA_SIGHT_ANGLE / 2, gridMapRectArea);
        Point edgeCellEnd = getPoint(rowStart, colStart, agvPose.getOrient() - GridMapConsts.CAMERA_SIGHT_ANGLE / 2, gridMapRectArea);
        List<Point> edgePoints = getEdgePoints(edgeCellStart, edgeCellEnd);
        // 连接边界栅格与agv pose点，判断连线是否碰到障碍物，碰到障碍物之前的栅格减值
        fillRectAreaWithPassThroughPoint(gridMapRectArea, rowStart, colStart, edgePoints);

        // 连接起点和障碍物点，判断连线是否碰到障碍物，碰到障碍物之前的栅格减值
//        List<Point> obPList = new ArrayList<>();
//        for (Point p : obstacleList) {
//            obPList.add(getObstaclePoint(gridMapRectArea, p, op));
//        }
//        fillRectAreaWithPassThroughPoint(gridMapRectArea, rowStart, colStart, obPList);

//        showGridMapPartArea(gridMapRectArea.getArea());
        return gridMapRectArea;
    }

    private void fillRectAreaWithPassThroughPoint(GridMapRectArea gridMapRectArea, int rowStart, int colStart, List<Point> edgePoints) {
        for (Point ep : edgePoints) {
            for (Point p : GridSimLine.getInstance().plotLine(colStart, rowStart, ep.getX(), ep.getY())) {
                if (checkNextObstacle(gridMapRectArea, p))
                    break;
                if (!inRange(p.getY(), gridMapRectArea.getArea().length)
                        || !inRange(p.getX(), gridMapRectArea.getArea()[p.getY()].length))
                    continue;
                gridMapRectArea.getArea()[p.getY()][p.getX()] = 1;
            }
        }
    }

    private boolean checkNextObstacle(GridMapRectArea gridMapRectArea, Point p) {
        int yMax = gridMapRectArea.getArea().length;
        int xMax = gridMapRectArea.getArea()[0].length;
        for (int i = -1; i <= 1; i++) {
            for (int j = -1; j <= 1; j++) {
                if (p.getY() + i >= 0 && p.getY() + i < yMax
                        && p.getX() + j >= 0 && p.getX() + j < xMax
                        && gridMapRectArea.getArea()[p.getY() + i][p.getX() + j] == GridMapConsts.GRID_OBSTACLE)
                    return true;
            }
        }
        return false;
    }

    private boolean inRange(int y, int length) {
        return y >= 0 && y < length;
    }

    private List<Point> getEdgePoints(Point edgeCellStart, Point edgeCellEnd) {
        return GridSimLine.getInstance().plotLine(edgeCellStart.getX(), edgeCellStart.getY(), edgeCellEnd.getX(), edgeCellEnd.getY());
    }

    private Point getPoint(int rowStart, int colStart, int orient, GridMapRectArea gridMapRectArea) {
        int row = rowStart - (int) (Math.sin(Math.toRadians(orient * 1.0 / 1000)) * GridMapConsts.CAMERA_SIGHT_LENGTH / getResolution());
        row = Math.max(row, 0);
        row = row >= gridMapRectArea.getArea().length ? gridMapRectArea.getArea().length - 1 : row;
        int col = colStart + (int) (Math.cos(Math.toRadians(orient * 1.0 / 1000)) * GridMapConsts.CAMERA_SIGHT_LENGTH / getResolution());
        col = Math.max(col, 0);
        col = col >= gridMapRectArea.getArea()[0].length ? gridMapRectArea.getArea()[0].length - 1 : col;
        return new Point(col, row);
    }

    private void fillObstacles(List<Point> obstacles, Point op, GridMapRectArea gridMapRectArea) {
        for (Point p : obstacles) {
            fillObstacle(gridMapRectArea, p, op);
        }
    }

    private void fillObstacle(GridMapRectArea gridMapRectArea, Point p, Point op) {
        int row = fitGridCell(op.getY() - p.getY()) / getResolution();
        int col = fitGridCell(p.getX() - op.getX()) / getResolution();
        row = Math.max(row, 0);
        col = Math.max(col, 0);
        row = Math.min(gridMapRectArea.getArea().length - 1, row);
        col = Math.min(gridMapRectArea.getArea()[0].length -1, col);
        gridMapRectArea.getArea()[row][col] = GridMapConsts.GRID_OBSTACLE;
    }

    private void getRectMap(Pose agvPose, int angleAntiClockwise, int angleClockwise, int size) {
        Point op;
        byte[][] area;
        if (atQuadrant1(angleAntiClockwise) && atQuadrant1(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX()), fitGridCell(agvPose.getY() + GridMapConsts.CAMERA_SIGHT_LENGTH));
            area = new byte[size][size];
        } else if (atQuadrant2(angleAntiClockwise) && atQuadrant2(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX() - GridMapConsts.CAMERA_SIGHT_LENGTH), fitGridCell(agvPose.getY() + GridMapConsts.CAMERA_SIGHT_LENGTH));
            area = new byte[size][size];
        } else if (atQuadrant3(angleAntiClockwise) && atQuadrant3(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX() - GridMapConsts.CAMERA_SIGHT_LENGTH), fitGridCell(agvPose.getY()));
            area = new byte[size][size];
        } else if (atQuadrant4(angleAntiClockwise) && atQuadrant4(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX()), fitGridCell(agvPose.getY()));
            area = new byte[size][size];
        } else if (atQuadrant1(angleAntiClockwise) && atQuadrant4(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX()), fitGridCell(agvPose.getY() + GridMapConsts.CAMERA_SIGHT_LENGTH));
            area = new byte[size * 2][size];
        } else if (atQuadrant2(angleAntiClockwise) && atQuadrant1(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX() - GridMapConsts.CAMERA_SIGHT_LENGTH), fitGridCell(agvPose.getY() + GridMapConsts.CAMERA_SIGHT_LENGTH));
            area = new byte[size][size * 2];
        } else if (atQuadrant3(angleAntiClockwise) && atQuadrant2(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX() - GridMapConsts.CAMERA_SIGHT_LENGTH), fitGridCell(agvPose.getY() + GridMapConsts.CAMERA_SIGHT_LENGTH));
            area = new byte[size * 2][size];
        } else if (atQuadrant4(angleAntiClockwise) && atQuadrant3(angleClockwise)) {
            op = new Point(fitGridCell(agvPose.getX() - GridMapConsts.CAMERA_SIGHT_LENGTH), fitGridCell(agvPose.getY()));
            area = new byte[size][size * 2];
        }
    }

    private int fitGridCell(int pa) {
        return pa - pa % GridMapConsts.GRID_LENGTH;
    }

    private boolean atQuadrant1(int angle) {
        return angle >= 0 && angle <= 90000;
    }

    private boolean atQuadrant2(int angle) {
        return angle >= 90000 && angle <= 180000;
    }

    private boolean atQuadrant3(int angle) {
        return angle >= 180000 && angle <= 270000;
    }

    private boolean atQuadrant4(int angle) {
        return angle >= 270000 && angle < 360000;
    }

    private byte[][] staticMap;
}
